/*
 * RangeFollower.cpp
 *
 *  Created on: Dec 4, 2012
 *      Author: bouchier
 */

#include <JRoverSensors.h>
#include "RangeFollower.h"

extern JRoverSensors jRoverSensors;

RangeFollower::RangeFollower() {
	flag = 0;
	layerName = (char *)"RangeFollower";
	rqVLinear = 10.0;
	rqVAngular = 0.0;

}

RangeFollower::~RangeFollower() {}

void RangeFollower::eval()
{
	int lInRange, cInRange, rInRange;
	int rangeVector;
	int l, c, r;			// left, center, right distance reading

	l = jRoverSensors.getLDistance();
	c = jRoverSensors.getCDistance();
	r = jRoverSensors.getRDistance();
	lInRange = (l<40 && l>16)?1:0;
	cInRange = (c<40 && c>16)?2:0;
	rInRange = (r<40 && r>16)?4:0;

	rangeVector = lInRange + cInRange + rInRange;
	switch(rangeVector) {
	case 0: flag = 0; break;	// if nothing in range, wait for someone to walk by
	case 2:						// drive toward the center if anything's in range of center sensor
	case 3:
	case 6:
	case 7:
		rqVAngular = 0.0;
		rqVLinear = (float)c;
		flag = 1;
		break;
	case 1:						// only left sensor sees something, so veer left
		rqVAngular = 0.5;
		rqVLinear = (float)l;
		flag = 1;
		break;
	case 4: 					// only right sensor sees something, veer right
		rqVAngular = -0.5;
		rqVLinear = (float)r;
		flag = 1;
		break;
	case 5:						// veer toward the closer of left or right sensor readings
		rqVAngular = (jRoverSensors.getLDistance() < jRoverSensors.getRDistance())? 0.5 : -0.5;
		rqVLinear = 10.0;
		flag = 1;
		break;
	}


}
